/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Timer;

/**
 *
 * @author James
 */
public class HopperController {
   
   Joystick joy1;     //Joystick duh
   Jaguar jag1;       //Jaguar obviously
   boolean start;     // starts feeding frisbees
   boolean s1Pressed; // press button for switch 1 to kick in
   boolean s2Pressed; // press button for switch 2 to kick in
   double cmd;        // how fast motor is going 
   int state;         // the different states the hopper would be in
   Timer time;        // time 
   double timeOut1;   // timeout into next state after starting
   double timeOut2;   // 2nd time timesout to get into 3rd state
   DigitalInput switch1; //switch1 making it the opposite direction
   DigitalInput switch2; //switch2 making the kicker stop
   public HopperController(Joystick joy1, Jaguar jag1)
   {
       this.joy1= joy1;
       this.jag1 = jag1;
       cmd = 0;           // cmd through s2Pressed is all just setting them to default settings
       state = 0;
       start = false;
       s1Pressed = false;
       s2Pressed = true; //suppose to be false but switch is working weird
       switch1 = new DigitalInput(1);
       switch2 = new DigitalInput(2);
      // timeOut1 = 5;     // first timeout starts after 5seconds
      // timeOut2 = 10;    // second timeout starts after 10secondb  
        time = new Timer(); //creates timer
       
   }
   
   public void control()
       {
           start = joy1.getRawButton(2); //sets start to limit switch 1
           s1Pressed = switch1.get(); // sets Switch 1 
           s2Pressed = switch2.get(); // sets Switch 2 to limit switch2
           
           switch(state){
               case 0:  //fist case, starting the whole hopper process
                   //System.out.println("S0"); checks to see what stage it's on
                   if(start == true)
                    {
                     cmd = 1; //sets the speed
                     state ++; //proceeds to next state/case
                     
                    } break;
               case 1:
                   //System.out.println("S1"); checks to see what stage it's on
                   if(s1Pressed == true)
                    {
                       cmd = -1; //sets the speed 
                       time.start(); 
                       state ++; //proceeds to next state/case
                    } break;
               case 2:
                  // System.out.println("S2"); checks to see what stage it's on
                   if(s2Pressed == false) //suppose to be true, but switch is working weird
                   {
                     cmd= 0; //resets speed to 0
                     time.stop();
                     state = 0; //resets the state/case to 0 and won't start until press A again
                     
                   } break;
           }
           
           jag1.set(cmd); //set jaguar to cmd
           
          // System.out.println("state " + state + " cmd "+ cmd); //prints the obvious thing you moron
            System.out.println("time " + time.get()); //same as above and I hate you for needing this comment
          //System.out.println("switch 1 " +switch1.get() + " switch 2 " +  switch2.get() );// seeing if switchs are working right
       
       }
   
   
}
